#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "GPS.h"
#include "com.h"

static char test_gps_string[256] =
	"$GPRMC,045306.90,A,3044.95369975,N,10355.45621938,E,0.130,139.4,050418,0.0,E,A*31";

static char test_heading_string[256] = "#HEADINGA,COM1,0,67.0,FINE,1995,363211.300,00000000,925914837,0;SOL_COMPUTED,NARROW_FLOAT,2.0052,126.2875,13.8394,0.0000,135.0683,121.1282,\"0\",9,8,8,6,0,00,0,c3*6a1237e0";

static char test_GPGGA_string[256] = "$GPGGA,124237.40,3044.94247979,N,10355.45478954,E,1,08,3.6,530.5193,M,-42.1549,M,00,0000*46";

static int gps_fd = 0;
static double heading_angle = 0.0;
static char rev_buff[256] = {0};
static struct gps current_gps;
int gps_update_flag = 0;

//度分格式转换成为度格式
double convert_dm2d(double dm)
{
	double d = dm / 100;
	int d_int = (int)d;
	double d_remain = (d - d_int) * 100.0 / 60.0;
	double reval = d_int + d_remain;
	return reval;
}
int gps_init(const char *port_name)
{
	gps_fd = uart_open_no_block(port_name);
	if (gps_fd != 0)
	{
		int err = uart_set(gps_fd, 38400, 0, 8, 1, 'N');
		if (0 == err)
		{
			printf("Open R60-G failed!\n");
			return 0;
		}
		else
		{
			printf("Open R60-G successful!\n");
			return 1;
		}
	}
}

static int get_gps_string(char gps_str[])
{
	static int i = 0;
	static char single_char = 0;
	if (uart_recv_no_block(gps_fd, &single_char, 1) > 0)
	{
		if (single_char == '$' || single_char == '#')
		{
			i = 0;
			rev_buff[i] = single_char;
			i++;
		}
		else
		{
			if (single_char == '\n' && i != 0)
			{
				rev_buff[i] = '\0';
				memcpy(gps_str, rev_buff, strlen(rev_buff) + 1);
				i = 0;
				return 1;
			}
			else
			{
				if (i > 0)
				{
					rev_buff[i] = single_char;
					i++;
				}
			}
		}
	}
	return 0;
}
//采用,或者;进行字符串的分割
int string_segment(char *buff_string, char strings[32][128])
{
	int i = 0;
	int j = 0;
	int k = 0;

	int len = strlen(buff_string);
	while (len--)
	{
		if (buff_string[k] != ',' && buff_string[k] != ';')
		{
			strings[i][j] = buff_string[k];
			k++;
			j++;
		}
		else
		{
			strings[i][j] = '\0';
			k++;
			i++;
			j = 0;
		}
	}
	return 1;
}
static void printf_strings(char strings[32][128])
{
	for (int i = 0; i < 32; ++i)
	{
		printf("%d %s\n", i, strings[i]);
	}
}
static int strings_to_GPRMC(char strings[32][128], struct gps *gps_ptr)
{
	if (NULL != strstr(strings[0], "$GPRMC"))
	{
		if (strings[2][0] == 'A')
		{
			gps_ptr->latitude = atof(strings[3]);
			gps_ptr->latitude = convert_dm2d(gps_ptr->latitude);

			gps_ptr->north_south = strings[4][0];

			gps_ptr->longitude = atof(strings[5]);
			gps_ptr->longitude = convert_dm2d(gps_ptr->longitude);
			gps_ptr->east_west = strings[6][0];

			gps_ptr->heading = atof(strings[8]);
			gps_ptr->declination = atof(strings[10]);

			return 1;
		}
		else
		{
			return 0;
		}
		return 1;
	}
	else
	{
		return 0;
	}
}

//$GPGGA,
//124237.60,
//3044.94248154 ,
//N,
//10355.45478868,
//E,
//1,
//08,
//3.6,
//530.5203,
//M,
//-42.1549,
//M,
//00,
//0000*48
static int strings_to_GPGGA(char strings[32][128], struct gps *gps_ptr)
{

	if (NULL != strstr(strings[0], "$GPGGA"))
	{
		gps_ptr->latitude = atof(strings[2]);
		gps_ptr->latitude = convert_dm2d(gps_ptr->latitude);

		gps_ptr->north_south = strings[3][0];

		gps_ptr->longitude = atof(strings[4]);
		gps_ptr->longitude = convert_dm2d(gps_ptr->longitude);

		gps_ptr->valid_flag = atoi(strings[6]);
		if (strings[13][0] == '0')
		{
			gps_ptr->diff_delay = strings[13][1] - '0';
		}
		else
		{
			gps_ptr->diff_delay = atoi(strings[13]);
		}
		if ((gps_ptr->valid_flag == 4 || gps_ptr->valid_flag == 5) && gps_ptr->diff_delay < 20)
		{
			return 1;
		}
		else
		{
			return 0;
		}
	}
	else
	{
		return 0;
	}
}

int get_gps(struct gps *gps_ptr)
{
	if (gps_update_flag == 1)
	{
		*gps_ptr = current_gps;
		return 1;
	}
	else
	{
		return 0;
	}
}
void printf_gps(const struct gps *gps_ptr)
{
	printf("GPS:");
	printf("%c", gps_ptr->north_south);
	printf("纬:%lf", gps_ptr->latitude);
	printf("%c", gps_ptr->east_west);
	printf("经:%lf", gps_ptr->longitude);
	printf(" valid:%d delay:%d\n", gps_ptr->valid_flag, gps_ptr->diff_delay);

	//printf("偏航%lf\n",gps_ptr->heading);
	//printf("地磁%lf\n",gps_ptr->declination);
}
static int strings_to_heading(char strings[32][128], double *heading_angle)
{
	if (NULL != strstr(strings[0], "#HEADINGA"))
	{
		*heading_angle = atof(strings[13]);
		return 1;
	}
	else
	{
		return 0;
	}
}
static int heading_update_flag = 0;
int get_heading_angle(double *heading)
{
	if (heading_update_flag == 1)
	{
		*heading = heading_angle;
		return 1;
	}
	else
	{
		return 0;
	}
}
static double heading = 0; //原始方向角
static double heading_old = 0;
static int cross_zero_times = 0;
static int first_time_flag = 0;
int get_continuous_heading_angle(double &continuous_heading)
{
	if (1 == heading_update_flag)
	{
		heading = heading_angle;
		if (0 == first_time_flag) //系统第一次采集到方向角时，让过去方向角的值和现在的方向角的值相等
		{
			heading_old = heading;
			first_time_flag = 1;
		}
		if (fabs(heading_old - heading) > 180) //相隔的两次方向采样点之间如果值突变超过180度，那么就认为过零点了
		{
			if (heading > heading_old) //逆时针方向穿过零点  1到359度
			{
				cross_zero_times -= 1;
			}
			else if (heading < heading_old) //顺时针方向穿过零点 359度到1度
			{
				cross_zero_times += 1;
			}
		}
		heading_old = heading; //更新最新的传感器数值到上一次值
		continuous_heading = heading + 360.0 * (double)cross_zero_times;
		return 1;
	}
	else
	{
		return 0;
	}
}

void dir2continuous_dir(const double continuous_heading, const double dir, double &continuous_dir)
{
	double continuous_heading_temp = continuous_heading;
	double dir_tem = dir;
	int circle_num = 0;
	while (fabs(continuous_heading_temp) >= 360.0)
	{
		continuous_heading_temp = fmod(continuous_heading_temp, 360.0);
		circle_num++;
	}
	if (continuous_heading > 0)
	{
		continuous_dir = dir + circle_num * 360.0;
	}
	else
	{
		continuous_dir = dir - circle_num * 360.0;
	}
}
static char gps_string[256];
int gps_update()
{
	if (gps_fd == 0)
	{
		return -1;
	}
	else
	{
		if (get_gps_string(gps_string) == 1)
		{

			char strings[32][128];
			string_segment(gps_string, strings);
			//if(strings_to_GPRMC(strings, &current_gps)==1)
			//{
			////printf_gps(&current_gps);
			//gps_update_flag = 1;
			//return 1;
			//}
			if (strings_to_GPGGA(strings, &current_gps) == 1)
			{
				//printf_gps(&current_gps);
				gps_update_flag = 1;
				return 2;
			}
			if (strings_to_heading(strings, &heading_angle) == 1)
			{
				//printf_gps(&current_gps);
				heading_update_flag = 1;
				return 1;
			}
		}
	}
	return 0;
}

int test_main()
{
	char strings[32][128];
	string_segment(test_gps_string, strings);
	if (strings_to_GPRMC(strings, &current_gps) == 1)
	{
		printf_gps(&current_gps);
		gps_update_flag = 1;
	}
	string_segment(test_heading_string, strings);
	if (strings_to_heading(strings, &heading_angle) == 1)
	{
		printf("heading:%lf\n", heading_angle);
		heading_update_flag = 1;
	}

	string_segment(test_GPGGA_string, strings);
	//printf_strings(strings);
	if (strings_to_GPGGA(strings, &current_gps) == 1)
	{
		printf_gps(&current_gps);
		gps_update_flag = 1;
	}
	return 0;
}
//ddmmm.mmm


